#pragma once

#define SIMPLE_MODEL 1

#define NUM_RANGEFINDERS 22
#define NUM_GOAL_SENSORS 2



#if SIMPLE_MODEL

#define NUM_INTERNAL_SENSORS 0 
#define NUM_ACTION_KNOB_SETTINGS 5 // 3 for turn rate (increase, decrease, stay the same) 2 for speed (forwards, backwards)


#define NUM_ACTIONS 6

#else
#define NUM_INTERNAL_SENSORS 2 // probably not subject to change -- just speed and turn rate
#define NUM_ACTION_KNOB_SETTINGS 6 // 3 for turn rate (increase, decrease, stay the same) 3 for speed
// Okay, do we want to enumerate each distinct outcome, and have a node for each one?
// No, nevermind, there are 9 total, lets stick with the 6 -- MDS

#define NUM_ACTIONS 9
#endif

#define NUM_SENSORS_FOR_CONTROLLER (NUM_INTERNAL_SENSORS+NUM_GOAL_SENSORS+NUM_RANGEFINDERS)

#define NUM_ITER_PER_TRAINING_RUN 400

#include "NeuralNetTwoLayers.h"
#include "NeuralNetThreeLayers.h"
#include "NeuralNetFourLayers.h"
#include "Layer.h"

#define USE_NET_PER_ACTION 0

typedef NeuralNetTwoLayers<		NnetLayer<NUM_SENSORS_FOR_CONTROLLER, 5, 1, 1>, \
								NnetLayer<5,1,1,1> > PerActionNnet;

//typedef NeuralNetThreeLayers<	NnetLayer<NUM_SENSORS_FOR_CONTROLLER, 5, 1, 1>, \
//								NnetLayer<5,3,1,1>, NnetLayer<3,1,1,1> > PerActionNnet;
//
//typedef NeuralNetTwoLayers<NnetLayer<NUM_SENSORS_FOR_CONTROLLER + NUM_ACTION_KNOB_SETTINGS, 5, 1, 1>, \
//							NnetLayer<5, 1, 1, 1> > ControllerNnet;


typedef NeuralNetThreeLayers<NnetLayer<NUM_SENSORS_FOR_CONTROLLER + NUM_ACTION_KNOB_SETTINGS, 15, 1, 1>, \
							NnetLayer<15, 5, 1, 1>, NnetLayer<5, 1, 1, 1> > \
							ControllerNnet;

//typedef NeuralNetFourLayers<NnetLayer<NUM_SENSORS_FOR_CONTROLLER + NUM_ACTION_KNOB_SETTINGS, 5, 1, 1>, \
//							NnetLayer<5, 3, 1, 1>, NnetLayer<3, 2, 1, 1>, NnetLayer<2, 1, 1, 1> > \
//							ControllerNnet;

class RobotController
{
	int m_nCurrSpeed;
	int m_nMaxSpeed;
	int m_nCurrTurnRate;
	int m_nMaxTurnRate;

	int m_nCurrIter;

	double m_arrLfSensors[NUM_SENSORS_FOR_CONTROLLER+NUM_ACTION_KNOB_SETTINGS]; 
	// need not be sensors per say, but state variables as well

	double m_arrlfSensorsForTraining[NUM_ITER_PER_TRAINING_RUN][NUM_SENSORS_FOR_CONTROLLER+NUM_ACTION_KNOB_SETTINGS];
	int m_arrNumActionSelected[NUM_ITER_PER_TRAINING_RUN];

	inline double * GetArrayForStep(int nStep)
	{
		if(nStep < 0)
		{
			return m_arrLfSensors;
		}
		else
		{
			return m_arrlfSensorsForTraining[nStep];
		}
		//int nInvalid = (nStep < 0);
		//return nInvalid*(&m_arrLfSensors[0]) +(!nInvalid)*(&m_arrlfSensorsForTraining[nStep][0]);
	}

	ControllerNnet m_Nnet;

	PerActionNnet m_arrNetsPerAction[NUM_ACTIONS];

	double m_arrLfActionValues[NUM_ACTIONS];

#if SIMPLE_MODEL

	inline int BoostSpeed(int nBy) { m_nCurrSpeed = (2*nBy-1);  return m_nCurrSpeed; }
	inline int BoostTurnRate(int nBy) { m_nCurrTurnRate = nBy; return m_nCurrTurnRate; }

	inline int ActionValToTurnKnob(int nNumAction) const { return (nNumAction>>1); }

	inline int ActionValToSpeedKnob(int nNumAction) const { return (nNumAction%2); }
#else
	inline int BoostSpeed(int nBy) { m_nCurrSpeed += nBy; ClampSpeed(); return m_nCurrSpeed; }
	inline int BoostTurnRate(int nBy) { m_nCurrTurnRate += nBy; ClampTurnRate(); return m_nCurrTurnRate; }

	inline int ActionValToTurnKnob(int nNumAction) const { return (nNumAction/3); }

	inline int ActionValToSpeedKnob(int nNumAction) const { return (nNumAction%3); }
#endif


	

public:
	RobotController(void);
	~RobotController(void);

	void ClampSpeed();
	void ClampTurnRate();
	inline void SetMaxSpeed(int nSpeed) { m_nMaxSpeed = nSpeed; ClampSpeed(); }
	inline void SetMaxTurnRate(int nTurnRate) { m_nMaxTurnRate = nTurnRate; ClampTurnRate(); }

	inline int GetSpeed() const { return m_nCurrSpeed; }
	inline int GetTurnRate() const { return m_nCurrTurnRate; }

	inline int BoostSpeed() { return BoostSpeed(1); }
	inline int BoostTurnRate() { return BoostTurnRate(1); }

	inline int ReduceSpeed() { return BoostSpeed(-1); }
	inline int ReduceTurnRate() { return BoostTurnRate(-1); }

	inline void ZeroInternalState() { m_nCurrSpeed = m_nCurrTurnRate = 0; }

	void GetRewardsFromState(int nTrainingIter);

	// void GetRewardsFromStateAndTrain(int nTrainingIter, double lfFutureDiscount, double lfTrainRate);

	int SelectActionProbabalistically(double lfBias, double lfMaxAnneal) const;
	
	int SelectMaxAction() const;

	inline double GetActionReward(int nAction) const {return m_arrLfActionValues[nAction]; }

	void SetRangeSensorData(int nTrainingIter, const double * arrLfSensorData);
	void SetGoalData(int nTrainingIter, double lfGoalParrallel, double lfGoalperp);
	void SetInternalSensors(int nTrainingIter);

	void Train(int m_nNumIters, double lfTrainRate, double lfDiscoutFactor, double lfValue, double lfNeutralValue);

	void TrainQLearning(int m_nNumIters, double lfTrainRate, double lfDiscoutFactor, double lfValue);

	void TrainOnSequence(int m_nNumIters, double lfTrainRate, double lfDiscoutFactor);

	void UpdateStateFromAction(int nAction);

	void SetActionForTraining(int nTrainingIter, int nAction);

	inline int GetMaxIter() const { return NUM_ITER_PER_TRAINING_RUN; }

	bool SaveNeuralNet(const char * szFileName);

	bool LoadNeuralNet(const char * szFileName);
};
